#ifndef SENSOR_FUSION_HPP
#define SENSOR_FUSION_HPP

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <Eigen/Dense>

class SensorFusion {
private:
    ros::NodeHandle nh_;
    ros::NodeHandle private_nh_;
    
    // 订阅器
    ros::Subscriber ndt_pose_sub_;
    ros::Subscriber rtk_sub_;
    ros::Subscriber imu_sub_;
    
    // 发布器
    ros::Publisher fused_pose_pub_;
    tf2_ros::TransformBroadcaster tf_broadcaster_;
    
    // 传感器数据
    geometry_msgs::PoseWithCovarianceStamped last_ndt_pose_;
    nav_msgs::Odometry last_rtk_pose_;
    sensor_msgs::Imu last_imu_data_;
    
    // 传感器权重
    double ndt_weight_;
    double rtk_weight_;
    double imu_weight_;
    
    // 传感器状态
    bool ndt_initialized_;
    bool rtk_initialized_;
    bool imu_initialized_;
    
    // 时间戳
    ros::Time last_fusion_time_;
    
public:
    SensorFusion();
    void ndtCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void rtkCallback(const nav_msgs::Odometry::ConstPtr& msg);
    void imuCallback(const sensor_msgs::Imu::ConstPtr& msg);
    void fuseData();
};

#endif // SENSOR_FUSION_HPP 